#include "ResponseSpectrumAnalysisTask.h"

#include "Utility/Algebra/Config.h"
#include "Utility/Algebra/sgArpack.h"
#include "Utility/Configuration/SGParameter.h"

#include "DBManager/IService/Result/Nodal/IACCELERATIONService.h"
#include "DBManager/IService/Result/Nodal/IDISPLACEMENTService.h"
#include "DBManager/IService/Result/Nodal/IVELOCITYService.h"

#include "SGFem/Pre/Assembly/Assembly.h"

#include "MModel.h"


#include "DBManager/IService/Input/Table/ITABDMP1Service.h"


using namespace Kernel;
using namespace SG::Algebra;
using namespace SG::DataStructure::Common;

const std::map<double, double>& safeGetSpectrum(int table_id, const std::map<int, std::map<double, double>>& allSpectra)
{
    auto it = allSpectra.find(table_id);
    if (it == allSpectra.end())
    {
        throw std::runtime_error("Spectrum data for Table ID " + std::to_string(table_id) + " not found in allSpectra map.");
    }
    return it->second;
}

std::map<double, double> getSpectrumForDamping(double                                       targetDamping,
                                               const std::map<double, int>&                   specselMap,
                                               const std::map<int, std::map<double, double>>& allSpectra)
{
    
    auto interpolateSpectrumValues = [](double frequency, const std::map<double, double>& spectrumData) -> double
    {
        if (spectrumData.empty())
        {
            return 0.0;
        }

        auto it_upper = spectrumData.lower_bound(frequency);

        if (it_upper == spectrumData.begin())
        {
            return it_upper->second;
        }
        if (it_upper == spectrumData.end())
        {
            return spectrumData.rbegin()->second;
        }

        auto   it_lower = std::prev(it_upper);
        double f1 = it_lower->first;
        double a1 = it_lower->second;
        double f2 = it_upper->first;
        double a2 = it_upper->second;

        return a1 + (a2 - a1) * (frequency - f1) / (f2 - f1);
    };

    if (specselMap.empty())
    {
        throw std::runtime_error("Spectrum selection map (specselMap) is empty.");
    }

    
    auto it_upper = specselMap.lower_bound(targetDamping);

    
    if (it_upper != specselMap.end() && it_upper->first == targetDamping)
    {
        return safeGetSpectrum(it_upper->second, allSpectra);
    }
  
    if (it_upper == specselMap.begin())
    {
        return safeGetSpectrum(it_upper->second, allSpectra);
    }
    
    if (it_upper == specselMap.end())
    {
        return safeGetSpectrum(specselMap.rbegin()->second, allSpectra);
    }

    
    auto   it_lower = std::prev(it_upper);
    double damp1 = it_lower->first;
    int    table_id1 = it_lower->second;
    double damp2 = it_upper->first;
    int    table_id2 = it_upper->second;

    const auto& spectrum1 = safeGetSpectrum(table_id1, allSpectra);
    const auto& spectrum2 = safeGetSpectrum(table_id2, allSpectra);

    std::map<double, double> interpolated_spectrum;

    
    for (const auto& pair : spectrum1)
    {
        double freq = pair.first;
        double accel1 = pair.second;
        
        double accel2 = interpolateSpectrumValues(freq, spectrum2);

        
        double interpolated_accel = accel1 + (accel2 - accel1) * (targetDamping - damp1) / (damp2 - damp1);
        interpolated_spectrum[freq] = interpolated_accel;
    }

    return interpolated_spectrum;
}

// ResponseSpectrumAnalysisTask

ResponseSpectrumAnalysisTask::ResponseSpectrumAnalysisTask(MModel&                                      model,
                                                           const SG::Utility::SGParameter&                parameter,
                                                           const SG::DataStructure::FEM::SPECSELData&   specselData,
                                                           const SG::DataStructure::FEM::TABDMP1Data&   tabdmp1Data)
    : MSolver(model, parameter), m_specselData(specselData), m_tabdmp1Data(tabdmp1Data)
{
}

ResponseSpectrumAnalysisTask::~ResponseSpectrumAnalysisTask()
{
}

void ResponseSpectrumAnalysisTask::Solve()
{
    int         ModesCount = myParameter.GetValue("ModesOrder").GetInt();      // Number of modes
    int         subcase = myParameter.GetValue("SubcaseID").GetInt();          // Subcase ID
    std::string combinationMethod = myParameter.GetValue("combinationMethod").GetString(); // Options: "SRSS", "CQC", "ABS"

    std::map<double, int> specselMap;
    for (const auto& spec_item : m_specselData.m_TABLESList)
    {
        specselMap[spec_item.m_DAMP] = static_cast<int>(spec_item.m_TID);
    }

    // Manual parsing of damping table is removed. Damping is now handled by m_tabdmp1Data.

    SpMat_t K;
    myModel->ComputeStiffnessMatrix(K);
    SpMat_t M;
    myModel->ComputeMassMatrix(M);

    int m = K.rows();
    int n = ModesCount;

    TArpack<Real_t> arpack;
    auto            opt = arpack.getDefaultOption();
    opt.nev = ModesCount;
    opt.which = "LM";
    arpack.setOption(opt);

    std::vector<Real_t> eigenvalues;
    std::vector<Vec_t>  eigenvectors;
    arpack.solve(K, M, eigenvalues, eigenvectors);

    Mat_t phi(m, n); // Eigenvector matrix
    Vec_t lambda(n); // Eigenvalue vector
    for (int i = 0; i < n; ++i)
    {
        phi.col(i) = eigenvectors[i];
        lambda(i) = eigenvalues[i];
    }

    for (int i = 0; i < n; ++i)
    {
        Vec_t  current_eigenvector = phi.col(i);
        Real_t generalized_mass = current_eigenvector.dot(M * current_eigenvector);
        if (generalized_mass > 1.0e-12)
        {
            Real_t scale_factor = 1.0 / std::sqrt(generalized_mass);
            phi.col(i) *= scale_factor;
        }
        else
        {
            std::cerr << "Warning: Mode " << i + 1 << " has zero or negative generalized mass. Skipping normalization." << std::endl;
        }
    }

    Vec_t r = Vec_t::Zero(m);

    int excitationDOF = myParameter.GetValue("SUPORT_C").GetInt();
    int excitationNodeID = myParameter.GetValue("SUPORT_ID").GetInt();
    int excitation_dof_index = 0;

    auto& assembler = SG::FEMSolver::AssemblyEigen::GetInstance();

    SG::DataStructure::FEM::DofSet dofs;
    dofs.push_back(static_cast<SG::DataStructure::FEM::DOFDirections>(excitationDOF - 1));

    std::vector<int> global_dof_ids;

    assembler.ComputeGlobalDOFId(excitationNodeID, dofs, global_dof_ids);

    if (!global_dof_ids.empty())
    {
        excitation_dof_index = global_dof_ids[0];
    }
    r(excitation_dof_index) = 1.0;

    Vec_t gama(n);
    gama = phi.transpose() * M * r;

    std::map<int, std::map<double, double>> allSpectra;

    std::vector<int> table_ids_to_load;
    for (const auto& pair : specselMap)
    {
        table_ids_to_load.push_back(pair.second);
    }

    for (const int tid : table_ids_to_load)
    {
        std::string freq_key = "TABLED1_FREQUENCIES_" + std::to_string(tid);
        std::string accel_key = "TABLED1_VALUES_" + std::to_string(tid);

        if (myParameter.Has(freq_key) && myParameter.Has(accel_key))
        {
            std::string freq_str = myParameter.GetValue(freq_key).GetString();
            std::string accel_str = myParameter.GetValue(accel_key).GetString();

            std::istringstream freq_iss(freq_str);
            std::istringstream accel_iss(accel_str);

            std::map<double, double> current_spectrum;
            double                   freq, accel;

            while (freq_iss >> freq && accel_iss >> accel)
            {
                current_spectrum[freq] = accel;
            }

            allSpectra[tid] = current_spectrum;
        }
        else
        {
            throw std::runtime_error("Spectrum data for ID " + std::to_string(tid) + " not found in myParameter.");
        }
    }

    std::vector<Vec_t> peak_modal_displacements(n);
    std::vector<Vec_t> peak_modal_velocities(n);
    std::vector<Vec_t> peak_modal_accelerations(n);
    Vec_t              natural_frequencies_hz(n);
    Vec_t              modal_damping_ratios(n);

    const double PI = 3.14159265358979323846;

    for (int i = 0; i < n; ++i)
    {
        auto omega = std::sqrt(std::max(0.0, lambda(i)));
        auto freq_hz = omega / (2.0 * PI);

        natural_frequencies_hz(i) = freq_hz;

        // Use the Interpolate function from the TABDMP1Data object
        double zeta = m_tabdmp1Data.Interpolate(freq_hz);
        modal_damping_ratios(i) = zeta;

        auto spectrum_for_mode = getSpectrumForDamping(zeta, specselMap, allSpectra);

        
        auto interpolateAccel = [](double frequency, const std::map<double, double>& spectrumData) -> double {
            if (spectrumData.empty()) return 0.0;
            auto it_upper = spectrumData.lower_bound(frequency);
            if (it_upper == spectrumData.begin()) return it_upper->second;
            if (it_upper == spectrumData.end()) return spectrumData.rbegin()->second;
            auto   it_lower = std::prev(it_upper);
            return it_lower->second + (it_upper->second - it_lower->second) * (frequency - it_lower->first) / (it_upper->first - it_lower->first);
        };

        double Sa = interpolateAccel(freq_hz, spectrum_for_mode);

        if (omega > 1.0e-9)
        {
            peak_modal_displacements[i] = gama(i) * phi.col(i) * (Sa / (omega * omega));
            peak_modal_velocities[i] = gama(i) * phi.col(i) * (Sa / omega);
            peak_modal_accelerations[i] = gama(i) * phi.col(i) * Sa;
        }
        else
        {
            peak_modal_displacements[i] = Vec_t::Zero(m);
            peak_modal_velocities[i] = Vec_t::Zero(m);
            peak_modal_accelerations[i] = Vec_t::Zero(m);
        }
    }

    SG::Algebra::Vec_t u_total(m), v_total(m), a_total(m);
    // SG::Algebra::Mat_t u_total(m), v_total(m), a_total(m);

    if (combinationMethod == "SRSS")
    {
        for (int i = 0; i < n; ++i)
        {
            u_total += peak_modal_displacements[i].cwiseAbs2();
            v_total += peak_modal_velocities[i].cwiseAbs2();
            a_total += peak_modal_accelerations[i].cwiseAbs2();
        }
        u_total = u_total.cwiseSqrt();
        v_total = v_total.cwiseSqrt();
        a_total = a_total.cwiseSqrt();
    }
    else if (combinationMethod == "NRL")
    {
        for (int k = 0; k < m; ++k)
        {
            int    dominant_mode_disp = -1;
            double max_response_disp = -1.0;
            for (int i = 0; i < n; ++i)
            {
                if (std::fabs(peak_modal_displacements[i](k)) > max_response_disp)
                {
                    max_response_disp = std::fabs(peak_modal_displacements[i](k));
                    dominant_mode_disp = i;
                }
            }

            double srss_sum_sq_disp = 0.0;
            for (int i = 0; i < n; ++i)
            {
                if (i != dominant_mode_disp)
                {
                    srss_sum_sq_disp += std::pow(peak_modal_displacements[i](k), 2);
                }
            }

            if (dominant_mode_disp != -1)
            {
                u_total(k) = max_response_disp + std::sqrt(srss_sum_sq_disp);
            }

            int    dominant_mode_vel = -1;
            double max_response_vel = -1.0;
            for (int i = 0; i < n; ++i)
            {
                if (std::fabs(peak_modal_velocities[i](k)) > max_response_vel)
                {
                    max_response_vel = std::fabs(peak_modal_velocities[i](k));
                    dominant_mode_vel = i;
                }
            }
            double srss_sum_sq_vel = 0.0;
            for (int i = 0; i < n; ++i)
            {
                if (i != dominant_mode_vel)
                {
                    srss_sum_sq_vel += std::pow(peak_modal_velocities[i](k), 2);
                }
            }
            if (dominant_mode_vel != -1)
            {
                v_total(k) = max_response_vel + std::sqrt(srss_sum_sq_vel);
            }

            int    dominant_mode_accel = -1;
            double max_response_accel = -1.0;
            for (int i = 0; i < n; ++i)
            {
                if (std::fabs(peak_modal_accelerations[i](k)) > max_response_accel)
                {
                    max_response_accel = std::fabs(peak_modal_accelerations[i](k));
                    dominant_mode_accel = i;
                }
            }
            double srss_sum_sq_accel = 0.0;
            for (int i = 0; i < n; ++i)
            {
                if (i != dominant_mode_accel)
                {
                    srss_sum_sq_accel += std::pow(peak_modal_accelerations[i](k), 2);
                }
            }
            if (dominant_mode_accel != -1)
            {
                a_total(k) = max_response_accel + std::sqrt(srss_sum_sq_accel);
            }
        }
    }
    else if (combinationMethod == "ABS")
    {
        for (int i = 0; i < n; ++i)
        {
            u_total += peak_modal_displacements[i].cwiseAbs();
            v_total += peak_modal_velocities[i].cwiseAbs();
            a_total += peak_modal_accelerations[i].cwiseAbs();
        }
    }
    else if (combinationMethod == "GRP")
    {
        
        const double close_freq_ratio = 0.1;

        for (int k = 0; k < m; ++k)
        {
            double u_sum_sq = 0.0;
            double v_sum_sq = 0.0;
            double a_sum_sq = 0.0;

            int i = 0;
            while (i < n)
            {
                int group_end = i;
                for (int j = i + 1; j < n; ++j)
                {
                    if (natural_frequencies_hz(j) > 1e-9 &&
                        (natural_frequencies_hz(j) - natural_frequencies_hz(j - 1)) / natural_frequencies_hz(j) < close_freq_ratio)
                    {
                        group_end = j;
                    }
                    else
                    {
                        break; // 当前组结束
                    }
                }

                double group_abs_sum_u = 0.0;
                double group_abs_sum_v = 0.0;
                double group_abs_sum_a = 0.0;
                for (int j = i; j <= group_end; ++j)
                {
                    group_abs_sum_u += std::fabs(peak_modal_displacements[j](k));
                    group_abs_sum_v += std::fabs(peak_modal_velocities[j](k));
                    group_abs_sum_a += std::fabs(peak_modal_accelerations[j](k));
                }

                u_sum_sq += std::pow(group_abs_sum_u, 2);
                v_sum_sq += std::pow(group_abs_sum_v, 2);
                a_sum_sq += std::pow(group_abs_sum_a, 2);
                
                i = group_end + 1;
            }

            u_total(k) = std::sqrt(u_sum_sq);
            v_total(k) = std::sqrt(v_sum_sq);
            a_total(k) = std::sqrt(a_sum_sq);
        }
    }
    else
    {
        throw std::runtime_error("Unknown modal combination method: " + combinationMethod);
    }

    for (int i = 0; i < n; ++i)
    {
        Real gMass = phi.col(i).transpose() * M * phi.col(i);
        Real gStiff = phi.col(i).transpose() * K * phi.col(i);
        myModel->SetEigenValue(i + 1, lambda[i], gMass, gStiff, 0, 0);
    }

    myModel->SetEigenVector(phi, lambda, subcase);

    myModel->SetDisplacement(u_total, subcase, 0.0);
    myModel->SetVelocity(v_total, subcase, 0.0);
    myModel->SetAcceleration(a_total, subcase, 0.0);
}